#include <rclcpp/rclcpp.hpp>
#include <xarm_api/xarm_ros_client.h>
#include <geometry_msgs/msg/twist.hpp>
#include "dh_gripper_ctl.hpp" // Updated include path

class TestNode : public rclcpp::Node {
public:
    TestNode() : Node("test_node") {
        xarm_node_ = rclcpp::Node::make_shared("vbppg_ros_client");

        gripper_control_subscription_ = this->create_subscription<geometry_msgs::msg::Twist>(
            "turtle1/cmd_vel", 10, std::bind(&TestNode::teleopCallback, this, std::placeholders::_1));
        xarm_client_ = std::make_shared<xarm_api::XArmROSClient>();
        xarm_client_->init(xarm_node_, hw_ns_);

        std::this_thread::sleep_for(std::chrono::milliseconds(500));
        hardware_ctl::init_gripper(xarm_client_);
        hardware_ctl::set_gripper_force(xarm_client_, 30);
        // hardware_ctl::close_gripper(xarm_client_);
        // hardware_ctl::open_gripper(xarm_client_);
    }

    void teleopCallback(const geometry_msgs::msg::Twist::SharedPtr msg)
    {
    if(msg->angular.z > 0.1) flag_ = true;
    if(msg->angular.z < -0.1) flag_ = false;
    std::cout << "flag: " << flag_ << std::endl;
    if(!flag_){
        hardware_ctl::open_gripper(xarm_client_);
    }
    else{
        hardware_ctl::close_gripper(xarm_client_);
    }
    }
    bool flag_=false;

private:

    std::shared_ptr<rclcpp::Node> xarm_node_;
    std::shared_ptr<xarm_api::XArmROSClient> xarm_client_;
    std::string hw_ns_ = "xarm";
    rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr gripper_control_subscription_;
};


int main(int argc, char **argv) {
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<TestNode>());
    rclcpp::shutdown();
    return 0;
}